package cs470.agent;

import Code.BZRClient;
import Code.CalculatePotentials;
import Code.Environment;
import Code.Point;
import Code.Tank;

public class PotentialFieldAgent extends AbstractAgent{

	private final static double ANGLULAR_ERROR_THRESHOLD = .2;
	private final static double SPEED_CONSTANT = .008;
	private final static double MINIMUM_GOING_DISTANCE = 6;
	
	private final static int SENSOR_THRESHOLD = 2;
	
	private double lastError = 0;
	
	private int tankId;
	
	private Environment environment;
	
	public PotentialFieldAgent(BZRClient client, Environment environment, int tankId) {
		super(client);
		this.environment = environment;
		this.tankId = tankId;
	}
	
	public void run() {
		Point lastLocation = new Point(0, 0);
		
		while(true) {
			
			final Tank myTank = getTank(tankId);
			
			Point potential;
			if(myTank.hasFlag()) {
				potential = environment.getPotentialForMyBase(tankId);
			} else {
				potential = environment.getPotential(tankId);
			}
			double myAngle = myTank.getAngle();
			double desiredAngle = CalculatePotentials.calcAngle(myTank.getLocation(), potential);
			double curError = myAngle - desiredAngle;
			double der = curError - lastError;
			double magnitude = myTank.getLocation().calcDistance(potential);
			
			double newAngle = curError + der;
			
			if(Math.abs(newAngle) < ANGLULAR_ERROR_THRESHOLD) {
				turn(0);
				drive(1 * magnitude * SPEED_CONSTANT + .9); 
			} else {
				if(magnitude > MINIMUM_GOING_DISTANCE) {
					drive(1 * magnitude * SPEED_CONSTANT + .8); 
				} else {
					drive(0); 
				}
				turn(newAngle * -.3); 
			}
			
			if(lastLocation.calcDistance(myTank.getLocation()) > SENSOR_THRESHOLD) {
				lastLocation = myTank.getLocation();
				
				//update occgrid
				environment.updateGrid(tankId);
			}
		}
	}
}
